#include "Prm.h"

Prm::Prm (	int number_vertices,
			int k_nearest,
			CollisionDetector* col,
			MultiRobotsSampler* sampler,
			ConfSet start,
			ConfSet target) :
	m_number_vertices(number_vertices),
	m_k_nearest(k_nearest), 
	m_col(col),
	m_sampler(sampler),
	m_start(start),
	m_target(target)
{
	m_graph = new Graph<int>(0,true);
	m_kd_tree = new Kd_tree_d<Kernel_d>();
}

Prm::~Prm()
{
	delete m_graph;
	delete m_kd_tree;
}

void Prm::generate_roadmap()
{
	for (int i = 0; i < m_number_vertices; ++i)
	{
		ConfSet configurations_set = m_sampler->generate_sample_no_obstacles();

		double temp_array[] = {CGAL::to_double(configurations_set[0].x()),
							   CGAL::to_double(configurations_set[0].y()),
							   CGAL::to_double(configurations_set[1].x()),
							   CGAL::to_double(configurations_set[1].y())};

		Point_d configuration_point(4, temp_array, temp_array + 4);

		m_kd_tree->insert(configuration_point);
		m_all_tree_points.push_back(configuration_point);
		m_graph->add_vertex(i);
	}

	int i = 0;
	for (vector<Point_d>::iterator current_point_it = m_all_tree_points.begin();
		current_point_it != m_all_tree_points.end();
		current_point_it++)
	{
		add_point_to_graph(*current_point_it, i++);
	}
}

void Prm::add_point_to_graph(Point_d p, int index)
{
	vector<Point_d> k_nearest_neighbors;

	m_kd_tree->k_nearest_neighbors(p, m_k_nearest, std::back_inserter(k_nearest_neighbors));

	ConfSet point_set = get_conf_set(p);

	for (vector<Point_d>::const_iterator k_nearest_neighbors_it = k_nearest_neighbors.begin();
		k_nearest_neighbors_it != k_nearest_neighbors.end();
		k_nearest_neighbors_it++)
	{
		int new_index = get_point_index(*k_nearest_neighbors_it);
		if(!m_graph->is_in_same_cc(index,new_index))
		{
			if (m_col->local_planner(point_set, get_conf_set(*k_nearest_neighbors_it), 10));
			{
				//m_graph->add_vertex(index);
				//m_graph->add_vertex(new_index);
				m_graph->add_edge(index, new_index);
			}
		}
	}
}

int Prm::get_point_index(Point_d p)
{
	int i = 0;

	for (vector<Point_d>::iterator all_tree_points_it = m_all_tree_points.begin();
		all_tree_points_it != m_all_tree_points.end();
		all_tree_points_it++)
	{
		if (*all_tree_points_it == p)
		{
			return i;
		}

		i++;
	}

	return 0;
}

void Prm::retrieve_path(vector<ConfSet>* path_vector_ptr)
{
	std::list<int> path;


	double start_config_array[] = {CGAL::to_double(m_start[0].x()),
								   CGAL::to_double(m_start[0].y()),
								   CGAL::to_double(m_start[1].x()),
								   CGAL::to_double(m_start[1].y())};

	int new_index = m_all_tree_points.size();

	Point_d start_configuration_point(4, start_config_array, start_config_array + 4);
	m_all_tree_points.push_back(start_configuration_point);
	m_graph->add_vertex(new_index);
	add_point_to_graph(start_configuration_point, new_index);

	double target_config_array[] = {CGAL::to_double(m_target[0].x()),
									CGAL::to_double(m_target[0].y()),
									CGAL::to_double(m_target[1].x()),
									CGAL::to_double(m_target[1].y())};

	Point_d target_configuration_point(4, target_config_array, target_config_array + 4);
	m_all_tree_points.push_back(target_configuration_point);
	m_graph->add_vertex(new_index + 1);
	add_point_to_graph(target_configuration_point, new_index + 1);
	m_graph->find_path(new_index,new_index + 1,path);

	vector<int> v;

	for (std::list<int>::iterator path_it = path.begin();
		path_it != path.end();
		++path_it)
	{
		path_vector_ptr->push_back(get_conf_set(m_all_tree_points[*path_it]));
		v.push_back(*path_it);
	}
}

ConfSet Prm::get_conf_set(Point_d point)
{
	ConfSet configuration;

	Point_2 robot_1_conf(
		CGAL::to_double(point[0]),
		CGAL::to_double(point[1]));

	configuration.push_back(robot_1_conf);

	Point_2 robot_2_conf(
		CGAL::to_double(point[2]),
		CGAL::to_double(point[3]));

	configuration.push_back(robot_2_conf);

	return configuration;
}